#include <opencv2/objdetect/objdetect.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include <iostream>
#include <stdio.h>
#include "opencv.hpp"

using namespace std;
using namespace cv;

#define EPSILON    (1.0E-3)
#define FRAME 1
#define MY_DEBUG 0
#define MY_DEBUG_DISPLAY 1
#define THRESHOLD_V 0.55
#define THRESHOLD_GESTURE 10
#define THRESHOLD_GESTURE_Y_AVE_FILTER 1

#define INTERVAL_TASK_1 3
#define RESIZE_TASK_1 0.05
#define FRAME_RESIZE_TASK_1 0  // the parameter to control if we need make the display frame bigger to show numbers.


#define RESIZE_TASK_2 0.3
#define REGION_TASK_2 10

#define RESIZE_TASK_3 0.1
#define REGION_TASK_3 10
#define TASK_3_FRAME_HISTORYS 4


Mat * lastFrame;
Mat * currentFrame;

Mat lastFrameMat;
Mat currentFrameMat;
void LKFunction(Mat &currentFrame, Mat &lastFrame,int interval, Mat &resultMat);
void LKFunction_4_Ix_Iy_It(Mat &currentFrame, Mat &lastFrame,int interval, Mat &resultMat);
void printOutTheMat(Mat &resultMat);
void printOutTheMat_Vec3f(Mat &resultMat,int xOffset,int yOffset,Mat &displayMat,float resize);
void drawArrow(IplImage *image, CvPoint p, CvPoint q, CvScalar color,int arrowMagnitude, int thickness, int line_type, int shift);
int task1(int argc, const char** argv) ;
int task2(int argc, const char** argv);
void printOutTheMat_Vec2f(Mat &resultMat,int xOffset,int yOffset,Mat &displayMat);
void drawArrow4TheMat_Vec2f(Mat &resultMat,int xOffset,int yOffset,Mat &displayMat,float resize,float displayRegion);

int getAverage(vector<float> & tempIxs);
int task3(int argc, const char** argv);
void LKFunction_4_Vx_Vy(Mat &currentFrame, Mat &lastFrame,int averageSize, Mat &resultMat);
void displayChannels4TheMat_Vec2f(Mat &resultMat,string windowNamePrefix);
void drawArrow4TheMat_Vec2f_Optimized(Mat &resultMat,int xOffset,int yOffset,Mat &displayMat,float resize, Mat * hisMat,float displayRegion);
void displayChannels4TheMat_Vec3f(Mat &resultMat);
void padding( Mat &resultMat,int regionSize);

//g++ LKIPCV.cpp -o XXX $O_LIBS
//g++ LKIPCV.cpp -o XXX $O_LIBS -stdlib=libstdc++   // for mac 10.9+

int main(int argc, const char** argv) {
//	return task1(argc,argv);
//	return task2(argc,argv);
	return task3(argc,argv);
}
int task1(int argc, const char** argv) {
	lastFrame = NULL;
	cv::VideoCapture cap;
	if (argc > 1) {
		cap.open(string(argv[1]));
	} else {
		cap.open(CV_CAP_ANY);
	}
	if (!cap.isOpened()) {
		printf("Error: could not load a camera or video.\n");
	}
	Mat frame;
	Mat resizedGrayFrame;
	namedWindow("video", 1);

	int i = 0;
	for (;;) {
		waitKey(20);
		cap >> frame;
		if (!frame.data) {
			printf("Error: no frame data.\n");
			break;
		}

		float colTemp = frame.cols;
		float heightTemp = RESIZE_TASK_1 * colTemp;
		float rowTemp = frame.rows;
		float widthTemp = RESIZE_TASK_1 * rowTemp;
	    resize(frame,resizedGrayFrame,Size(round(heightTemp),round(widthTemp)));

	    cvtColor( resizedGrayFrame, resizedGrayFrame, CV_RGB2GRAY );
		GaussianBlur(resizedGrayFrame, resizedGrayFrame, Size(9, 9), 0, 0);
		if(FRAME_RESIZE_TASK_1){
			heightTemp = colTemp*FRAME_RESIZE_TASK_1;
			widthTemp = rowTemp*FRAME_RESIZE_TASK_1;
			resize(frame,frame,Size(round(heightTemp),round(widthTemp)));
		}
		i++;
		if (i % FRAME == 0) {
			if(MY_DEBUG){
				cout << i << endl;
			}
			if (lastFrame == NULL) {
				lastFrame = &frame;
//				lastFrameMat = frame.clone();
			    cvtColor( frame, lastFrameMat, CV_BGR2GRAY );
				continue;
			}



			if (lastFrame == NULL) {
				lastFrame = &resizedGrayFrame;
				lastFrameMat = resizedGrayFrame.clone();
				continue;
			}
			currentFrameMat = resizedGrayFrame.clone();

			Mat resultMat(lastFrameMat.rows,lastFrameMat.cols,CV_32FC3,Scalar::all(0));//Scalar(1,2,3)
			LKFunction_4_Ix_Iy_It(currentFrameMat, lastFrameMat,INTERVAL_TASK_1,resultMat);
			if(FRAME_RESIZE_TASK_1){
				float resizeBack = (1/RESIZE_TASK_1)*FRAME_RESIZE_TASK_1;
				printOutTheMat_Vec3f(resultMat,INTERVAL_TASK_1/2,INTERVAL_TASK_1/2,frame,resizeBack);
				displayChannels4TheMat_Vec3f(resultMat);
			}else{
				float resizeBack = 1/RESIZE_TASK_1;
				printOutTheMat_Vec3f(resultMat,INTERVAL_TASK_1/2,INTERVAL_TASK_1/2,frame,resizeBack);
				displayChannels4TheMat_Vec3f(resultMat);
			}

			lastFrameMat = resizedGrayFrame.clone();
//			break;//only do 2 frames;
		}
		if (i > 10000) {
			i = 0;
		}

		imshow("video", frame);
	}


	namedWindow("currentFrame", 1);
	imshow("currentFrame", currentFrameMat);
	namedWindow("lastFrame", 1);
	imshow("lastFrame", lastFrameMat);

	waitKey(0);
	return -1;
}


int task2(int argc, const char** argv) {
	lastFrame = NULL;
	cv::VideoCapture cap;
	if (argc > 1) {
		cap.open(string(argv[1]));
	} else {
		cap.open(CV_CAP_ANY);
	}
	if (!cap.isOpened()) {
		printf("Error: could not load a camera or video.\n");
	}
	Mat frame;
	Mat tempframe;
	Mat tempframe2;
	namedWindow("video", 1);

	int i = 0;
	for (;;) {
		waitKey(20);
		cap >> frame;

		if (!frame.data) {
			printf("Error: no frame data.\n");
			break;
		}
		float colTemp = frame.cols;
		float heightTemp = RESIZE_TASK_2 * colTemp;
		float rowTemp = frame.rows;
		float widthTemp = RESIZE_TASK_2 * rowTemp;
	    resize(frame,tempframe,Size(round(heightTemp),round(widthTemp)));

	    cvtColor( tempframe, tempframe2, CV_RGB2GRAY );
		GaussianBlur(tempframe2, tempframe2, Size(9, 9), 0, 0);

		i++;
		if (i % FRAME == 0) {
			if(MY_DEBUG){
				cout << i << endl;
			}
			if (lastFrame == NULL) {
				lastFrame = &tempframe;
				lastFrameMat = tempframe2.clone();
				continue;
			}
			currentFrameMat = tempframe2.clone();
			Mat resultMat(lastFrameMat.rows,lastFrameMat.cols,CV_32FC2,Scalar::all(0));//Scalar(1,2,3)
//			Mat temp = resultMat.clone();
			LKFunction_4_Vx_Vy(currentFrameMat, lastFrameMat,REGION_TASK_2,resultMat);
			float resizeBack = 1/RESIZE_TASK_2;
			drawArrow4TheMat_Vec2f(resultMat,REGION_TASK_2/2,REGION_TASK_2/2,frame,resizeBack,REGION_TASK_2);
			displayChannels4TheMat_Vec2f(resultMat,"");
			padding(resultMat,REGION_TASK_2);
			displayChannels4TheMat_Vec2f(resultMat,"padded_");
			lastFrameMat = tempframe2.clone();
			imshow("video", frame);
		}
		if (i > 10000) {
			i = 0;
		}

	}

	namedWindow("currentFrame", 1);
	imshow("currentFrame", currentFrameMat);
	namedWindow("lastFrame", 1);
	imshow("lastFrame", lastFrameMat);

	waitKey(0);
	return -1;
}


int flaggg = 0;
int task3(int argc, const char** argv) {
	lastFrame = NULL;
	cv::VideoCapture cap;
	if (argc > 1) {
		cap.open(string(argv[1]));
	} else {
		cap.open(CV_CAP_ANY);
	}
	if (!cap.isOpened()) {
		printf("Error: could not load a camera or video.\n");
	}
	Mat frame;
	Mat tempframe;
	Mat tempframe2;
	namedWindow("video", 1);

	Mat hisMat[TASK_3_FRAME_HISTORYS];
//	hisMat[0] = NULL;
//	hisMat[1] = NULL;
//	hisMat[2] = NULL;

	int i = 0;
	for (;;) {
		waitKey(20);
		cap >> frame;

		if (!frame.data) {
			printf("Error: no frame data.\n");
			break;
		}
		float colTemp = frame.cols;
		float heightTemp = RESIZE_TASK_3 * colTemp;
		float rowTemp = frame.rows;
		float widthTemp = RESIZE_TASK_3 * rowTemp;
	    resize(frame,tempframe,Size(round(heightTemp),round(widthTemp)));

	    cvtColor( tempframe, tempframe2, CV_RGB2GRAY );
		GaussianBlur(tempframe2, tempframe2, Size(9, 9), 0, 0);

		i++;
		if (i % FRAME == 0) {
			if(MY_DEBUG){
				cout << i << endl;
			}
			if (lastFrame == NULL) {
				lastFrame = &tempframe;
				lastFrameMat = tempframe2.clone();
				continue;
			}
			currentFrameMat = tempframe2.clone();
			//do LKFnction

			Mat resultMat(lastFrameMat.rows,lastFrameMat.cols,CV_32FC2,Scalar::all(0));//Scalar(1,2,3)
			Mat temp = resultMat.clone();
			LKFunction_4_Vx_Vy(currentFrameMat, lastFrameMat,REGION_TASK_3,temp);
			float resizeBack = 1/RESIZE_TASK_3;
			drawArrow4TheMat_Vec2f_Optimized(temp,REGION_TASK_3/2,REGION_TASK_3/2,frame,resizeBack,hisMat,REGION_TASK_3);

			lastFrameMat = tempframe2.clone();
			imshow("video", frame);
		}
		if (i > 10000) {
			i = 0;
		}

	}

	namedWindow("currentFrame", 1);
	imshow("currentFrame", currentFrameMat);
	namedWindow("lastFrame", 1);
	imshow("lastFrame", lastFrameMat);

	waitKey(0);
	return -1;
}


void padding( Mat &resultMat,int regionSize){
	int row = resultMat.rows - regionSize;
	int col = resultMat.cols - regionSize;
	for (int i = 0; i < row; i = i + regionSize) {//TODO change to 1
		for (int j = 0; j < col; j = j + regionSize) {
			float Vx = resultMat.at<Vec2f>(i, j)[0];
			float Vy = resultMat.at<Vec2f>(i, j)[1];
			if(MY_DEBUG){
				cout << "V=" << Vx;
				cout << "," << Vy << "\t";
			}
			for (int k = 1; k < regionSize; k++) { // I just paded the
				for (int m = 1; m < regionSize; m++) {
					resultMat.at<Vec2f>(i+k, j+m)[0] = Vx;
					resultMat.at<Vec2f>(i+k, j+m)[1] = Vy;
				}
			}
		}
		if(MY_DEBUG){
			cout << endl;
		}
	}
}


/*
 * LKFunction for given rigon.
 *
 */
void LKFunction_4_Vx_Vy(Mat &currentFrame, Mat &lastFrame,int regionSize, Mat &resultMat){
	Mat tempMat(lastFrameMat.rows,lastFrameMat.cols,CV_32FC3,Scalar::all(0));
	LKFunction_4_Ix_Iy_It(currentFrameMat, lastFrameMat,1,tempMat);
	int row = currentFrame.rows - regionSize;
	int col = currentFrame.cols - regionSize;
	for (int i = 0; i < row; i = i + regionSize) {//TODO change to 1
		for (int j = 0; j < col; j = j + regionSize) {
//			vector<float> tempIxs;
//			vector<float> tempIys;
//			vector<float> tempIts;
			float A1 = 0;
			float A2 = 0;
			float A3 = 0;
			float A4 = 0;
			float b1 = 0;
			float b2 = 0;


			for (int k = 0; k < regionSize; k = k + 1) {
				for (int m = 0; m < regionSize; m = m + 1) {
					int y = i + k;
					int x = j + m;
					float ix = tempMat.at<Vec3f>(y, x)[0];
					float iy = tempMat.at<Vec3f>(y, x)[1];
					float it = tempMat.at<Vec3f>(y, x)[2];

//					cout << "ix = " << ix;
//					cout << ", iy = " << iy;
//					cout << ", it = " << it << endl;
					A1 = A1 + ix * ix;
					A2 = A2 + ix * iy;
					A3 = A2;
					A4 = A4 + iy * iy;

					b1 = b1 + it * ix;
					b2 = b2 + it * iy;
				}
			}

			b1 = 0-b1;
			b2 = 0-b2;

			/*
			 *======    =======
			 *======  A =======
			 *======    =======
			 */
			Mat tempMatA = Mat(2,2,CV_32FC1);
			tempMatA.at<float>(0, 0) = A1;
			tempMatA.at<float>(1, 0) = A2;
			tempMatA.at<float>(0, 1) = A3;
			tempMatA.at<float>(1, 1) = A4;

			/*
			 *======    =======
			 *======  A -1 =======
			 *======    =======
			 */
			Mat tempMatA_1 = Mat(2,2,CV_32FC1);
			tempMatA_1 = tempMatA.inv();

			/*
			 *======    =======
			 *======  b =======
			 *======    =======
			 */
			Mat tempMatb = Mat(2,1,CV_32FC1);
			tempMatb.at<float>(0) = b1;
			tempMatb.at<float>(1) = b2;

			/*
			 *======    =======
			 *====== V  =======
			 *======    =======
			 */
			Mat V = tempMatA_1 * tempMatb;

			float Vx = V.at<float>(0);
			float Vy = V.at<float>(1);

			resultMat.at<Vec2f>(i, j)[0] = Vx;
			resultMat.at<Vec2f>(i, j)[1] = Vy;

//			float A_M = A1*A4 - A2*A3;//|A| = ad-db = A1*A4 - A2*A3
//			cout << "i = " << i;
//			cout << ", j = " << j;
			if(MY_DEBUG){
				cout << "V=" << Vx;
				cout << "," << Vy << "\t";

			}

		}
		if(MY_DEBUG){
			cout << endl;
		}
	}
}

/*
 * display values of 3 channels on the given image.
 */
void printOutTheMat_Vec3f(Mat &resultMat,int xOffset,int yOffset,Mat &displayMat,float resize=1){
	int row = resultMat.rows;
	int col = resultMat.cols;
	for (int i = 0; i < row; i++) {
		for (int j = 0; j < col; j++) {
			float ix = resultMat.at<Vec3f>(i, j)[0];
			float iy = resultMat.at<Vec3f>(i, j)[1];
			float it = resultMat.at<Vec3f>(i, j)[2];
			if(fabs(ix - 0) < EPSILON && fabs(iy - 0) < EPSILON && fabs(it - 0) < EPSILON){
				continue;
			}
			if(MY_DEBUG){
				cout << i <<","<<j<<endl;
			}

			int x = (j + xOffset) * resize;
			int y = (i + yOffset) *resize;

			string temp;
			stringstream ss;
			ss<<ix<<",";
			ss<<iy<<",";
			ss<<it;

			putText(displayMat, ss.str(), cvPoint(x,y), FONT_HERSHEY_SIMPLEX, 0.3, cvScalar(0,255,0), 1, CV_AA);
		}
	}
}


/*
 *
 * Draw arrows on the display mat with vectors contained by the resultmat
 */
void drawArrow4TheMat_Vec2f(Mat &resultMat,int xOffset,int yOffset,Mat &displayMat,float resize,float displayRegion){
	int row = resultMat.rows;
	int col = resultMat.cols;

	for (int i = 0; i < row; i++) {
		for (int j = 0; j < col; j++) {
			float vx = resultMat.at<Vec2f>(i, j)[0];
			float vy = resultMat.at<Vec2f>(i, j)[1];
			if(fabs(vx - 0) < EPSILON && fabs(vy - 0) < EPSILON){
				continue;
			}

			if(MY_DEBUG){
				string temp;
				stringstream ss;
				ss<<vx<<",";
				ss<<vy;
	//			ss<<i<<",";
	//			ss<<j;
	//			cout << i <<","<<j<<endl;
				//putText(displayMat, ss.str(), cvPoint(j + xOffset,i + yOffset), FONT_HERSHEY_SIMPLEX, 0.3, cvScalar(200,200,250), 1, CV_AA);
			}

			float x = j + xOffset;
			float y = i + yOffset;
//			drawArrow(IplImage(displayMat), cvPoint(x, y), cvPoint(x + vx *20, y + vy *20), CV_RGB(100, 0, 255));


			IplImage srcx = IplImage(displayMat);
			float px1 = x * resize;
			float py1 = y *resize;
			float px2 = (x + vx *displayRegion/2) *resize;
			float py2 = (y + vy *displayRegion/2) *resize;


//			int x = j * resize;
//			int y = i *resize;
//			int px1 = x + xOffset;
//			int py1 = y + yOffset;
////			drawArrow(IplImage(displayMat), cvPoint(x, y), cvPoint(x + vx *20, y + vy *20), CV_RGB(100, 0, 255));
//
//
//			IplImage srcx = IplImage(displayMat);
////			int px1 = x * resize;
////			int py1 = y *resize;
//			int px2 = x + (vx *displayRegion/2) *resize;
//			int py2 = y + (vy *displayRegion/2) *resize;

			int arrowMagnitue = displayRegion;
			int thickness = 3;
			if(arrowMagnitue > 8){
				arrowMagnitue = displayRegion;
				thickness = 3;
			}else if(arrowMagnitue == 1){
				arrowMagnitue = 5;
				thickness = 1;
			}else{
				arrowMagnitue = 5;
				thickness = 2;
			}

			drawArrow(&srcx, cvPoint(px1, py1), cvPoint(px2, py2), CV_RGB(100, 0, 255),arrowMagnitue,thickness,8,0);
		}
	}
}

/*
 *
 * Draw arrows on the display mat with vectors contained by the resultmat. -- optimized(anti-noise)
 */
void drawArrow4TheMat_Vec2f_Optimized(Mat &resultMat,int xOffset,int yOffset,Mat &displayMat,float resize,Mat * hisMatxx,float displayRegion){
	int row = resultMat.rows;
	int col = resultMat.cols;

//	Mat *hisMat0 = hisMat;
//	Mat *hisMat1 = hisMat + 1 ;
//	Mat *hisMat2 = hisMat + 2 ;
//	*hisMat1 = hisMat2->clone();
//	*hisMat0 = hisMat1->clone();
//	*hisMat2 = resultMat.clone();


	Mat * hisMat[TASK_3_FRAME_HISTORYS];
	for(int i = 0 ; i < TASK_3_FRAME_HISTORYS ; i++){
		hisMat[i] = hisMatxx + i;
	}

	for(int i = 0 ; i < TASK_3_FRAME_HISTORYS-1 ; i++){
		*(hisMat[i]) = hisMat[i+1]->clone();
	}
	*(hisMat[TASK_3_FRAME_HISTORYS-1]) = resultMat.clone();


	if(flaggg < TASK_3_FRAME_HISTORYS){
		flaggg++;
		return;
	}

	float resultVarx = 0;
	float resultVary = 0;
	float pointCouter = 0;

	for (int i = 0; i < row; i++) {
		for (int j = 0; j < col; j++) {
			float vx = resultMat.at<Vec2f>(i, j)[0];
			float vy = resultMat.at<Vec2f>(i, j)[1];
			if(fabs(vx - 0) < EPSILON && fabs(vy - 0) < EPSILON){
				continue;
			}

			if(MY_DEBUG){
				string temp;
				stringstream ss;
				ss<<vx<<",";
				ss<<vy;
	//			ss<<i<<",";
	//			ss<<j;
	//			cout << i <<","<<j<<endl;
				//putText(displayMat, ss.str(), cvPoint(j + xOffset,i + yOffset), FONT_HERSHEY_SIMPLEX, 0.3, cvScalar(200,200,250), 1, CV_AA);
			}

			int x = j + xOffset;
			int y = i + yOffset;
//			drawArrow(IplImage(displayMat), cvPoint(x, y), cvPoint(x + vx *20, y + vy *20), CV_RGB(100, 0, 255));




			IplImage srcx = IplImage(displayMat);
			int px1 = x * resize;
			int py1 = y *resize;
			int px2 = (x + vx *REGION_TASK_3/2) *resize;
			int py2 = (y + vy *REGION_TASK_3/2) *resize;



//			float vx0 = hisMat0->at<Vec2f>(i, j)[0];
//			float vy0 = hisMat0->at<Vec2f>(i, j)[1];
//			float vx1 = hisMat1->at<Vec2f>(i, j)[0];
//			float vy1 = hisMat1->at<Vec2f>(i, j)[1];
//			float vx2 = hisMat2->at<Vec2f>(i, j)[0];
//			float vy2 = hisMat2->at<Vec2f>(i, j)[1];

			float vxs[TASK_3_FRAME_HISTORYS];
			float vys[TASK_3_FRAME_HISTORYS];
			for(int k = 0 ; k < TASK_3_FRAME_HISTORYS ; k++){
				vxs[k] = hisMat[k]->at<Vec2f>(i, j)[0];
				vys[k] = hisMat[k]->at<Vec2f>(i, j)[1];
			}




			int vxcount = 0;
			int vycount = 0;

//			if(vx0 > 0){
//				vxcount++;
//			}
//			if(vx1 > 0){
//				vxcount++;
//			}
//			if(vx2 > 0){
//				vxcount++;
//			}

			for(int k = 0 ; k < TASK_3_FRAME_HISTORYS ; k++){
				if(vxs[k] > 0){
					vxcount++;
				}
				if(vys[k] > 0){
					vycount++;
				}
			}



			if(MY_DEBUG){
				if(i == 0 && j ==0){
					if(vx > 0){
						cout << i<<","<< j<< ",vx0 > 0," << "vxcount="<<vxcount << ",vx=" <<vx;
						cout << ",vxs=";
						for(int k = 0 ; k < TASK_3_FRAME_HISTORYS ; k++){
							cout << k << "=" << vxs[k]<<";";
						}
						cout << endl;
					}else{
						cout  << i<< j<< ",vx0 <= 0," << vxcount << ",vx=" <<vx  <<endl;
					}
				}

				cout << i<<","<< j<<"="<< vxcount<<endl;
			}


			//antinoise

			int arrowMagnitue = displayRegion;
			int thickness = 3;
			if(arrowMagnitue > 8){
				arrowMagnitue = displayRegion;
				thickness = 3;
			}else if(arrowMagnitue == 1){
				arrowMagnitue = 5;
				thickness = 1;
			}else{
				arrowMagnitue = 5;
				thickness = 2;
			}

//			if(vxcount == 1 || vxcount == 2){
			if(vxcount != 0 && vxcount != TASK_3_FRAME_HISTORYS && vycount != 0 && vycount != TASK_3_FRAME_HISTORYS){
				if( (fabs(vx) < THRESHOLD_V) || (fabs(vy) < THRESHOLD_V) ){
					if(MY_DEBUG_DISPLAY){
						drawArrow(&srcx, cvPoint(px1, py1), cvPoint(px2, py2), CV_RGB(0, 255, 0),arrowMagnitue,thickness,8,0);
					}
				}
				continue;
			}


			if(fabs(vx) < THRESHOLD_V && fabs(vy) < THRESHOLD_V ){
				if(MY_DEBUG_DISPLAY){
					drawArrow(&srcx, cvPoint(px1, py1), cvPoint(px2, py2), CV_RGB(255, 0, 0),arrowMagnitue,thickness,8,0);
				}
				continue;
			}

			drawArrow(&srcx, cvPoint(px1, py1), cvPoint(px2, py2), CV_RGB(100, 0, 255),arrowMagnitue,thickness,8,0);
			resultVarx = resultVarx + vx;
			resultVary = resultVary + vy;
			pointCouter = pointCouter + 1;
		}
	}
	float gap = fabs(fabs(resultVarx) - fabs(resultVary));
	gap = gap*pointCouter;
	cout << "average gap="<<gap <<endl;

	float aveY = fabs(resultVary)/pointCouter;

//	if(fabs(resultVarx) > THRESHOLD_GESTURE && gap<THRESHOLD_GESTURE_X_Y_GAP){//THRESHOLD_GESTURE_X_Y_GAP

	if(fabs(resultVarx) > THRESHOLD_GESTURE && aveY < THRESHOLD_GESTURE_Y_AVE_FILTER){//THRESHOLD_GESTURE_X_Y_GAP
		if(resultVarx > 0){
			cout << "right" <<endl;
			putText(displayMat, "right", cvPoint(200,200), FONT_HERSHEY_SIMPLEX, 5, cvScalar(200,200,250), 5, CV_AA);
		}else{
			cout << "left" <<endl;
			putText(displayMat, "left", cvPoint(200,200), FONT_HERSHEY_SIMPLEX, 5, cvScalar(200,200,250), 5, CV_AA);
		}
	}

}

//float maxX = 0;
//float minX = 0;
//float maxY = 0;
//float minY = 0;

/*
 * display all channels with grey level images separately.  (2 channels only)
 */
void displayChannels4TheMat_Vec2f(Mat &resultMat,string windowNamePrefix = ""){
	GaussianBlur(resultMat, resultMat, Size(9, 9), 0, 0);

	Mat xMat(resultMat.rows,resultMat.cols,CV_8UC1,Scalar::all(0));
	Mat yMat(resultMat.rows,resultMat.cols,CV_8UC1,Scalar::all(0));
	int row = resultMat.rows;
	int col = resultMat.cols;
	float maxX = 0;
	float minX = 0;
	float maxY = 0;
	float minY = 0;
	for (int i = 0; i < row; i++) {
		for (int j = 0; j < col; j++) {
			float vx = resultMat.at<Vec2f>(i, j)[0];
			if (vx >= maxX) {
				maxX = vx;
			}
			if (vx <= minX) {
				minX = vx;
			}
		}
	}

	for (int i = 0; i < row; i++) {
		for (int j = 0; j < col; j++) {
			float vy = resultMat.at<Vec2f>(i, j)[1];
			if (vy >= maxY) {
				maxY = vy;
			}
			if (vy <= minY) {
				minY = vy;
			}
		}
	}
	//instead of getting he max min in frame, we fixed this max and min, because the real max and min are various,
	//could be very big or very small, therefore if we get max and min in realtime, the output video will flick.
	// (the extreme big value will make it bright and small will cause dark effect )
	//After long time observation, we found that the major values are in this range -100 ~ 155. so we fixed it.
	// So, by fixing max and min we can ignore the values which are toooo big or tooooo small.
//	float maxX = 155;
//	float minX = -100;
//	float maxY = 155;
//	float minY = -100;

	if(MY_DEBUG){
		printf("*****************");
		printf("****maxX=%f****minX=%f*********",maxX,minX);
		printf("****maxY=%f****minY=%f*********",maxY,minY);
		printf("*****************\n");
	}

	for (int i = 0; i < row; i++) {
		for (int j = 0; j < col; j++) {
			float vx = resultMat.at<Vec2f>(i, j)[0];
			vx = vx - minX;
			//vx = vx *1000;

			double xxsum = (255 / (maxX - minX)) * vx;
			xMat.at<uchar> (i, j) = (uchar) xxsum;
		}
	}

	namedWindow(windowNamePrefix + "Xcomponent", 1);
	imshow(windowNamePrefix + "Xcomponent", xMat);

	for (int i = 0; i < row; i++) {
		for (int j = 0; j < col; j++) {
			float vy = resultMat.at<Vec2f>(i, j)[1];
			vy = vy - minY;
			//vx = vx *1000;
			float temp = (float)255;
			float xxsum = (temp / (maxY - minY)) * vy;
			yMat.at<uchar> (i, j) = (uchar) xxsum;
		}
	}

	namedWindow(windowNamePrefix + "Ycomponent", 1);
	imshow(windowNamePrefix + "Ycomponent", yMat);

}


/*
 * display all channels with grey level images separately.  (3 channels only)
 */
void displayChannels4TheMat_Vec3f(Mat &resultMat){
	GaussianBlur(resultMat, resultMat, Size(9, 9), 0, 0);

	Mat xMat(resultMat.rows,resultMat.cols,CV_8UC1,Scalar::all(0));
	Mat yMat(resultMat.rows,resultMat.cols,CV_8UC1,Scalar::all(0));
	Mat zMat(resultMat.rows,resultMat.cols,CV_8UC1,Scalar::all(0));

	int row = resultMat.rows;
	int col = resultMat.cols;

	float maxX = 0;
	float minX = 0;
	float maxY = 0;
	float minY = 0;
	float maxZ = 0;
	float minZ = 0;

	for (int i = 0; i < row; i++) {
		for (int j = 0; j < col; j++) {
			float vx = resultMat.at<Vec3f>(i, j)[0];
			if (vx >= maxX) {
				maxX = vx;
			}
			if (vx <= minX) {
				minX = vx;
			}
		}
	}

	for (int i = 0; i < row; i++) {
		for (int j = 0; j < col; j++) {
			float vy = resultMat.at<Vec3f>(i, j)[1];
			if (vy >= maxY) {
				maxY = vy;
			}
			if (vy <= minY) {
				minY = vy;
			}
		}
	}



	for (int i = 0; i < row; i++) {
		for (int j = 0; j < col; j++) {
			float vz = resultMat.at<Vec3f>(i, j)[2];
			if (vz >= maxZ) {
				maxZ = vz;
			}
			if (vz <= minZ) {
				minZ = vz;
			}
		}
	}

	//instead of getting he max min in frame, we fixed this max and min, because the real max and min are various,
	//could be very big or very small, therefore if we get max and min in realtime, the output video will flick.
	// (the extreme big value will make it bright and small will cause dark effect )
	//After long time observation, we found that the major values are in this range -100 ~ 155. so we fixed it.
	// So, by fixing max and min we can ignore the values which are toooo big or tooooo small.
//	float maxX = 155;
//	float minX = -100;
//	float maxY = 155;
//	float minY = -100;
//	float maxZ = 155;
//	float minZ = -100;

	if(MY_DEBUG){
		printf("*****************");
		printf("****maxX=%f****minX=%f*********",maxX,minX);
		printf("****maxY=%f****minY=%f*********",maxY,minY);
		printf("****maxZ=%f****minZ=%f*********",maxZ,minZ);
		printf("*****************\n");
	}

	for (int i = 0; i < row; i++) {
		for (int j = 0; j < col; j++) {
			float vx = resultMat.at<Vec3f>(i, j)[0];
			vx = vx - minX;
			//vx = vx *1000;
			float maxGrayValue = (float)255;
			double normalizedx = (maxGrayValue / (maxX - minX)) * vx;
			xMat.at<uchar> (i, j) = (uchar) normalizedx;
		}
	}

	namedWindow("Xcomponent", 1);
	imshow("Xcomponent", xMat);

	for (int i = 0; i < row; i++) {
		for (int j = 0; j < col; j++) {
			float vy = resultMat.at<Vec3f>(i, j)[1];
			vy = vy - minY;
			//vx = vx *1000;
			float maxGrayValue = (float)255;
			float normalizedy = (maxGrayValue / (maxY - minY)) * vy;
			yMat.at<uchar> (i, j) = (uchar) normalizedy;
		}
	}

	namedWindow("Ycomponent", 1);
	imshow("Ycomponent", yMat);


	for (int i = 0; i < row; i++) {
		for (int j = 0; j < col; j++) {
			float vz = resultMat.at<Vec3f>(i, j)[2];
			vz = vz - minZ;
			//vx = vx *1000;
			float maxGrayValue = (float)255;
			float normalizedz = (maxGrayValue / (maxZ - minZ)) * vz;
			zMat.at<uchar> (i, j) = (uchar) normalizedz;
		}
	}

	namedWindow("Tcomponent", 1);
	imshow("Tcomponent", zMat);

}

//to work out the ix iy it
void LKFunction_4_Ix_Iy_It(Mat &currentFrame, Mat &lastFrame,int interval, Mat &resultMat) {
	int row = currentFrame.rows-interval;
	int col = currentFrame.cols-interval;
	for (int i = 0; i < row; i = i + interval) {
		for (int j = 0; j < col; j = j + interval) {
			int tempi = i;
			int tempj = j;
//			cout << "tempi = " << tempi << endl;
//			cout << "tempj = " << tempj << endl;
			uchar ca1 = currentFrame.at<uchar>(tempi, tempj);
			uchar ca2 = currentFrame.at<uchar>(tempi+interval, tempj);
			uchar ca3 = currentFrame.at<uchar>(tempi, tempj+interval);
			uchar ca4 = currentFrame.at<uchar>(tempi+interval, tempj+interval);

			tempi = i;
			tempj = j;
			uchar la1 = lastFrame.at<uchar>(tempi, tempj);
			uchar la2 = lastFrame.at<uchar>(tempi+interval, tempj);
			uchar la3 = lastFrame.at<uchar>(tempi, tempj+interval);
			uchar la4 = lastFrame.at<uchar>(tempi+interval, tempj+interval);

			//A
			float IxA1 =  ca1 - ca3;
			float IxA2 =  ca2 - ca4;
			float IxA3 =  la1 - la3;
			float IxA4 =  la2 - la4;

			float IyA1 =  ca1 - ca2;
			float IyA2 =  ca3 - ca4;
			float IyA3 =  la1 - la2;
			float IyA4 =  la3 - la4;

			float ItA1 = la1 - ca1; ///TODO
			float ItA2 = la2 - ca2;
			float ItA3 = la3 - ca3;
			float ItA4 = la4 - ca4;

			float IxA = (IxA1 + IxA2 + IxA3 + IxA4)/4;
			float IyA = (IyA1 + IyA2 + IyA3 + IyA4)/4;
			float ItA = (ItA1 + ItA2 + ItA3 + ItA4)/4;

			if(MY_DEBUG){
				cout << IxA;
				cout << "," << IyA;
				cout << "," << ItA << "\t";
			}


			resultMat.at<Vec3f>(i, j)[0] = IxA;
			resultMat.at<Vec3f>(i, j)[1] = IyA;
			resultMat.at<Vec3f>(i, j)[2] = ItA;
//			float xxx = resultMat.at<Vec3f>(i, j)[0];
//			cout << "xxx = " << xxx << endl;
//			printf("%f\n",xxx);
		}

		if(MY_DEBUG){
			cout  << endl;
		}
	}

	//padding
	for(int i = row ; i < currentFrame.rows ; i++){
		for(int j = 0 ; j < currentFrame.cols ; j++){
			resultMat.at<Vec2f>(i, j)[0] = resultMat.at<Vec2f>(row - 1, j)[0];
			resultMat.at<Vec2f>(i, j)[1] = resultMat.at<Vec2f>(row - 1, j)[1];
		}
	}

	for(int i = 0 ; i < currentFrame.rows ; i++){
		for(int j = col ; j < currentFrame.cols ; j++){
			resultMat.at<Vec2f>(i, j)[0] = resultMat.at<Vec2f>(i, col - 1)[0];
			resultMat.at<Vec2f>(i, j)[1] = resultMat.at<Vec2f>(i, col - 1)[1];
		}
	}
}

//drawArrow
void drawArrow(IplImage *image, CvPoint p, CvPoint q, CvScalar color, int arrowMagnitude = 9, int thickness=1, int line_type=8, int shift=0)
{
	//int arrowMagnitude, int thickness, int line_type, int shift
    //Draw the principle line
    cvLine(image, p, q, color, thickness, line_type, shift);
    const double PI = 3.141592653;
    //compute the angle alpha
    double angle = atan2((double)p.y-q.y, (double)p.x-q.x);
    //compute the coordinates of the first segment
    p.x = (int) ( q.x +  arrowMagnitude * cos(angle + PI/4));
    p.y = (int) ( q.y +  arrowMagnitude * sin(angle + PI/4));
    //Draw the first segment
    cvLine(image, p, q, color, thickness, line_type, shift);
    //compute the coordinates of the second segment
    p.x = (int) ( q.x +  arrowMagnitude * cos(angle - PI/4));
    p.y = (int) ( q.y +  arrowMagnitude * sin(angle - PI/4));
    //Draw the second segment
    cvLine(image, p, q, color, thickness, line_type, shift);
}
